//
// Created by kai-xu on 2023/2/21.
//

#ifndef ALIGN_MATRIX_H_
#define ALIGN_MATRIX_H_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <exception>
#include <vector>

#include "common/basic_type.h"
namespace autodrive {
namespace perception {

struct AlignMatrixParameters {
  std::vector<float> net_range;
  std::int32_t sd;
  std::vector<float> voxel_size;
};

template <typename T>
void gen_align_matrix(Eigen::Matrix4d const& pre_matrix,
                      Eigen::Matrix4d const& cur_matrix,
                      AlignMatrixParameters const& para, std::int16_t scale,
                      std::int32_t maxSize, float max_value,
                      T* const out) noexcept(false) {
  auto const& sd{para.sd};
  auto const& vs{para.voxel_size};
  auto const& netRange{para.net_range};
  constexpr float quantifyOffset{0.5};

  if (0 == sd) {
    throw std::runtime_error("sd is 0");
  }

  float x_stride{vs[0] * sd};    // 0.8
  float y_stride{vs[1] * sd};    // 0.8
  float x_offset{x_stride / 2};  // 0.4
  float y_offset{y_stride / 2};  // 0.4

  if (0 == x_stride) {
    throw std::runtime_error("x_stride is 0");
  }

  if (0 == y_stride) {
    throw std::runtime_error("y_stride is 0");
  }

  auto const& width{
      static_cast<int32_t>(round((netRange[3] - netRange[0]) / x_stride))};
  auto const& height{
      static_cast<int32_t>(round((netRange[4] - netRange[1]) / y_stride))};

  Eigen::Matrix4d const& pose_matrix{pre_matrix.inverse() * cur_matrix};

  Eigen::Matrix3d feat2bev;
  feat2bev << x_stride, 0, netRange[0], 0, y_stride, netRange[1], 0, 0, 1;

  Eigen::Matrix3d const& feat2bevInv{feat2bev.inverse()};
  Eigen::Matrix3d new_pose_matrix{Eigen::Matrix3d::Identity()};
  new_pose_matrix.block(0, 0, 2, 2) = pose_matrix.block(0, 0, 2, 2);
  new_pose_matrix(0, 2) = pose_matrix(0, 3);
  new_pose_matrix(1, 2) = pose_matrix(1, 3);

  Eigen::Matrix3d const& tf{feat2bevInv * new_pose_matrix * feat2bev};

  for (int32_t i{0}; i < height; i++) {
    for (int32_t j{0}; j < width; j++) {
      double tnp1 = tf(0, 0) * j + tf(0, 1) * i + tf(0, 2);
      double tnp2 = tf(1, 0) * j + tf(1, 1) * i + tf(1, 2);

      double tnp1_filter = (tnp1 / (width - 1)) * 2.0 - 1.0;
      double tnp2_filter = (tnp2 / (height - 1)) * 2.0 - 1.0;

      tnp1_filter = (tnp1_filter > max_value) ? max_value : tnp1_filter;
      tnp2_filter = (tnp2_filter > max_value) ? max_value : tnp2_filter;

      tnp1_filter = (tnp1_filter < -max_value) ? -max_value : tnp1_filter;
      tnp2_filter = (tnp2_filter < -max_value) ? -max_value : tnp2_filter;

      auto const& xIndex{i * width * 2 + j * 2 + 0};
      auto const& yIndex{i * width * 2 + j * 2 + 1};

      if (xIndex >= maxSize) {
        continue;
      }

      if (yIndex >= maxSize) {
        continue;
      }

      out[xIndex] = quantify<double, T>(tnp1_filter, scale, quantifyOffset);
      out[yIndex] = quantify<double, T>(tnp2_filter, scale, quantifyOffset);
    }
  }
}
}  // namespace perception
}  // namespace autodrive
#endif  // ALIGN_MATRIX_INCLUDE_ALIGH_POSE_MATRIX_H_